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1. INTRODUCTION 

The automation field had a sharp increase in the amount of robotic industrialization, many 
autonomous mobile robots had been developed such as unmanned aerial vehicles (UAVs) and unmanned 
ground vehicles (UGVs). These robots are to support the human in many difficult missions and working in 
various environments, those tasks can be rescued scans, spaces, surgery or military. In general, almost the 
architecture of an autonomous robot is divided into two parts as the master part and the slave part. The slave 
side includes the human operator, computer, and controller devices which were directed interaction with 
humans. The master side includes the robot frame and the actuators integrated with a system of sensors such 
as the Lidar sensor, visual camera, and optical sensor. In this model, the human operator either sends the 
command to the master side via a wired or wireless network. 

Normally, the human operator only sees the UAV and environment on the monitor based on the 
sensor and the visual feedback. Therefore, the tactile information needs to append to improve the feeling of 
the human operator. The haptic technology had seemed a key by the purpose is aim to bring more experience 
feeling about the robot working environment to the operator controller. For example, the personal haptic 
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interface mechanism (PHANToM) [1], [2] and Novint Falcon [3], [4] devices enable are used to control the 
manipulator as well as feel the environment around the workspace of the robot. With similar functions of the 
two above devices, we selected the Novint Falcon to develop the control algorithms. 

Novint Falcon haptic was used in many research and real applications. For example, the haptic 
presented the interaction forces between the excavator and soil [5], control position and velocity for the 3- 
DOF manipulator [6], make position control for the 6DOF Denso VM6083 robot arm [7]. The realistic force 
feedback in cardiac surgery [8], [9]. Falcon haptic also integrated with a laser sensor to applied in a 
wheelchair with the purpose is support blind people [10], and the nuclear industry [11]. For some 
applications used the haptic to make feedback force to the user to help avoid obstacles is as in [12], [13]. In 
the UAV field, the end-effector of Falcon haptic is used to set velocity command for the UAV [14], [15], and 
the force feedback to haptic is to help the user avoiding obstacles that closed the UAV [16]-[18]. In some 
critical cases, the end-effector position of the Falcon is used for changing the angle of UAV motors using the 
inverse kinematic equation [19]. 

To apply the algorithms to the haptic device, the kinematic structure and dynamic configuration 
needs to be carefully considered as in some previous researches [20]-[23]. Herein, the inverse kinematic is 
expressed for calculating the angle of motors based on the know the end-effector position. In this case, the 
dynamic model is to determine the torques of the motors based on the Jacobian and force applied to the grip. 
It is easy to see that if we know the target position of the end-effector then we could control the angle of each 
motor correspond with this position. However, the system often exits more disturbance the motor disturbance 
(motor load, stiffness and damping), environmental conditions and error of mechanical. Therefore, the 
inverse kinematic will not provide an accurate position if only uses the equation by the angle motors. 
Therefore, the controller is designed to maintain the stability as well as the accuracy of the system. The 
proportional integral derivative (PID) controller [24], [25] is widely used to control the mechanism in 
industrial with the loop feedback, which calculates the error between the desired value and feedback value, 
and then it makes the control signal to the mechanism. Especially, PID control is useful to control the motion 
of the system [26]. In addition, the microchip and the personal computer need the sampling time to get the 
measured value from the system to make the feedback signal. Thus, this paper is aiming to develop a position 
controller which is to increment the accuracy of the position control for the end-effector position of the haptic 
device based on a discrete-time PID controller. 

The paper is organized as follows: firstly, we are going to present the model configuration and the 
invest kinematic of the Novint Falcon haptic device in section 2. Secondly, the dynamics configuration as the 
Jacobian matrix is described in section 3 for obtained the actuator torques based on the end-effector 
trajectory. Then, the details of the proposed controller are presented in section 4. After that, in section 5 we 
experimented with the position controller with an attitude and heading reference system (AHRS) sensor 
integrated with the grip to estimate the end-effector position, the result had demonstrated the performance of 
the proposed controller. Finally, the conclusions are given in section 6. 


2. NOVINT FALCON HAPTIC DEVICE 

The geometry of the Novint Falcon was developed by Novint Technologies Company [27] as shown 
in Figure 1 [21]. The model design of Falcon has included three parts such as a moving platform, a base 
platform and three parallel linkages to connect the two above platforms. Three motors are mounted on the 
base platform. The operator can manually control the robot by moving the end-effector in the Falcon haptic 
and three internal motors could be made some force to the operator. 


a 


Figure 1. Novint falcon haptic hardware 
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Figures 2 and 3 represent one leg and base of the Novint Falcon haptic, respectively. A reference 
coordinate XYZ is attached at point O which is denoted the center of the base platform. The radius of the base 
platform is r and the distance from the end-effector to the highest joint E is denoted c. The length of the 
associated between joints AB, BC, CD and DE are a, e, b and d, respectively. The Novint Falcon dimensions 
are shown detail in Table 1. The end-effector position denoted by point P in the XYZ fixed coordinate frame 
and a local coordinate frame UVW are attached to the fixed point A. Three angles 6,;,02;,03; E R? with 
i=(1, 2, 3) represent each joint A, B and C, respectively. 


Table 1. The dimension of parameters in Novint Falcon [28] 
Parameters Value (mm) 
60.0 
102.5 
15.7 
115 
11.5 
26.2 
27.9 
36.6 
27.2 
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Figure 3. Novint Falcon platform (a) base platform, and (b) moving platform 


The inverse kinematic is to calculate the three angles 01i, 02i, 03i of the motors using the known 
location of the end-effector P(P,, Py, P, )which can present in the UVW coordinate frame by the homogeneous 
transformation matrix as in (1). 


b, coso, sing, 0|| P. =r 
P, |=| -sin g; cosg, 0 || P, |+| 0 (1) 
P; 0 0 1j P 0 
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where the g, is the angle from the center of Novint Falcon haptic to each axis i=(1,2,3), which has the values 
as —,—and =, respectively. 


The angle actuators 61i, 02i, 83; can be obtained by [15]: 


6, = 2atan(y,,) (2) 
ere. P;—acos i +c (3) 
d +e+bsin 0, 
P, 
DE (2) (4) 


with term y,; can be determined from (5). 
LW thu ty; =0 (5) 
In which 
l; = P} +P? +2cp,, —2ap,, —b° sin(O,,)° — 2be sin(@,, ) — 2bd sin(@,, ) (6) 
-2de-2ac +8 +c? -d° -e 


l; =—4ap,, (7) 


L; = P} +P} + 2cp,, +2ap,, —b’ sin(O,,)” —2be sin(O,,) — 2bd sin(@,, ) 


wi ui 


patie tee -e (8) 


Following (2), (3) and (4), the position of the end-effector could be obtained by controlling the angle 
of the motors. However, the system often has disturbances that come from the noise of motors, and the 
environment. The inverse kinematic cannot provide accurately the position of the end-effector which could 
have trouble if it combines with the robotic systems. Therefore, in this paper, we propose the position control 
for the haptic device based on the discrete-time PID. 


3. DYNAMICS CONFIGURATION 
The Jacobian basic function can be defined as in (9). 


x=Jq (9) 


Where x is end-effector position and orientation; q is the set of joint angles; J is the Jacobian matrix. Let’s 
define as in (10), (11) [17], 


Jy Jo Ji y; y 2 T 
J=|Jq Ja Jn [:4=[9: 42 9] mdsl v, vT (10) 
Jy Ja Jaz 
J cos(ġ, )cos(0,; )sin (6, i)- sin(¢,)cos(0, a 
j asin@, sin(8,, -6,,) 
i. sin(¢,)cos(0,,) sin(0, ,)+cos(¢,)cos(8, ) aD 
# asin@,,sin(8,, -0 ) 
sin(,, )sin(,,) 
J3 


t asin@,,sin(6,, -9,;) 
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with i, j=(1,2,3) and Oi are in (2), (3) and (4). Applying formula of 
tas "F 


where F is the vector combining end-effector forces and moments; J is Jacobian matrix; T is actuator 
torques. Therefore, the actuator torques can be obtained by the force of the end-effector. 


4. PROPOSED DISCRETE-TIME PID CONTROLLER 
In this section, a discrete-time PID controller is developed to drive the position of the Novint Falcon 


haptic device, which has the structure in Figure 4. 
Discrete-time Novint Falcon 
PID haptic device 


Grip position (X,Y,Z) 


Figure 4. The diagram of position control proposal 


Let’s consider the continuous PID form in (12). 


de(t) 
dt 


(12) 


u(t) = K,e(t)+K, Í e(r)dt+K, 


Where the error e(t) is defined by subtraction between the desired input value and the measured output value, 
u(t) is the controller output. Three parameters Kp, Kg, and K; are defined as proportional gain, derivative gain 
and integral gain, respectively. 

Talking the Z transform of (12), one has 


Ts Z+1 -1 +bz~t+cz~? 

UZ) = (Kp + K+ Ky S)e@ =e (13) 
de(t)) z-11 me z+1T. . T, Kı T, „Ka. 
in which z|“ |~* ~~ F(z); es sa=K_ +K =+ b=-K +K =—-2 ; 

(e e R bee 


c= “a and T, is the sampling period. 
From (13), we have (14). 


U(z) = U(z)z™t + aE (z) + bz™tE(z) + cz™?E(z) (14) 


The time-domain of (14) can be presented as in (15). From (15), the PIDz can be applied to any system with 
the gains K,,, K;, Ka and sampling time Ts. 


u(k) = u(k — 1) + ae(k) + be(k — 1) + ce(k — 2) (15) 


5. EXPERIMENT AND RESULTS 

The experiment is performed to demonstrate the proposed position control, which includes the 
Novint Falcon haptic device connected to one desktop PC via the USB connection. The PC is installed in the 
Linux OS, the AHRS sensor is integrated with the grip to estimate the end-effector position to configure the 
input/output for the Novint device. Herein, the input is the force and the output is the position of the end- 
effector. The configuration of the experiment is shown in Figure 5. 
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The discrete-time PID controller in Sec. 4 is a built-in Linux OS PC using the C++ program. In all 
simulations, we selected the sampling time 7,=0.0//s]. And the gains of a PID controller can be obtained by 
the trial and error method. By using this method, the K4 and K; are set to zero for the first time. We increase 
the K, to the response becomes faster and the output of the loop oscillates. This step needs careful because 
the system could unstable if the K, is very large. After that, K; is increased to stop the oscillations and reduce 
the steady-state error but the overshoot may appear. In the last step, Ka is increased to cancel the error which 
is created by Kj. Finally, we can select the gain of the PID controller as K,=1.5, Kg=5, Ki=0.0013. To prove 
the performance of the proposed controller, two scenarios are implemented in cases without the disturbance 
and with disturbance to the end-effector by the human forces. 


Linux PC Novint Falcon 


Discrete-time | Force 
PID controller 


Integrated to the grip 


Grip position (X, Y, Z) 


Figure 5. The configuration of experiment 


In the first scenario, the initial of Novint Haptic is random position as P,=1.5 [cm], Py=-5.7 [cm], 
P.=4.3 [cm] and the desired position is Py=2.5 [cm], Py-=-2.5 [cm], P,=4.0 [cm]. As a result in Figure 6, 
the position of haptic could meet the desired position at 7.2 /s] and keep stability during the time working 
which can show very detail in Figure 7. Therein, all position errors (e;=Pi,-P; where i=x, y, z) come to zeros 
and maintain at this value. These results show that the discrete-time PID proposed control can provide a 
solution for controlling the end-effector position to any desired trajectory with high accuracy and ensure the 
stability system. 


fs NOVINT HAPTIC POSITION NOVINT HAPTIC POSITION ERROR 
Er Error Px | 4 
ab Error Py 
tc Error Pz 
ele = 
7 banrer il £ 
5 Desired Py c 
5 o Desired Pz £ 
Q pA 
3 3 
= -2f Pz 2 
I wW 
all 
6 à 4 4 4 4 l l. L 4 1 
0 2 4 6 8 10 12 14 16 18 0 2 4 6 8 10 12 14 16 18 
Time [s] Time [s] 
Figure 6. The position of end-effector without Figure 7. The error position of end-effector without 
disturbance disturbance 


In the second scenario, a disturbance from human force is effected to the end-effector in the random 
time at 6.5/s], I1[s], 14.9[s], 19[s], 22[s] and 26.5[s] to all directions of the grip. The result in Figure 8 
showed that the discrete-time PID can maintain the stability of the position end-effector haptic to the desired 
position notwithstanding the disturbance from any direction. The detailed result is shown in Figure 9 
demonstrated that although the disturbances in this situation are quite large, the proposed controller still 
maintains the stability for the haptic device. The results proved convincingly that the position control performance 
could be ensured by using our proposed control with large noises and disturbances in environment. 
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Figure 8. The position of end-effector with external Figure 9. The error position of end-effector with 
forces external forces 


6. CONCLUSION 

This paper presents the discrete-time PID controller to drive the end-effector position of Novint 
Falcon haptic. Herein, the inverse kinematic and dynamic models are carefully considered and show some 
limitations of the control position. Instead of using the inverse kinematic equation to take the end-effector to 
go to the desired point, the proposed position control is suggested based on the PID controller. As a result, the 
grip of Novint Falcon could follow the desired trajectory quickly with high accuracy in disturbance conditions. 
The proposed control could provide good performance for the haptic device to apply any robotics system. 
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